
#include <agv_peripheral_controller/ioLift/io_lifter_controller.h>
#include <agv_srvs/srv/output_interface.hpp>

namespace agv_peripheral
{

    CIoLift::CIoLift()
    {
    }

    CIoLift::~CIoLift()
    {
    }

    bool CIoLift::init(ros::ServiceClient &client)
    {
        m_client = client;
        return true;
    }

    // 举升上升
    bool CIoLift::up()
    {
        bool finish = false;
        if (ctrl.size() == 0 || ctrl.size() > 2)
        {
            return finish;
        }
        // 如果到达上限位，停止完成此动作
        if (upTarget())
        {
            finish = stop();
            DEBUG_STREAM_OUT("CIoLift:up target:key=" << ctrl[0].key
                                                    << ",set stop finish=" << finish);
            return finish;
        }

        // 控制上升输出

        // 控制停止输出
        if (ctrl.size() == 2)
        {
            ctrl[0].value = true;
            ctrl[1].value = false;
        }
        else
        {
            ctrl[0].value = true;
        }
        bool ret = outputIo(ctrl);
        DEBUG_OUT("up:set io out key=" << ctrl[0].key << ",ret=" << ret);
        return finish;
    }

    // 举升下降
    bool CIoLift::down()
    {
        bool finish = false;
        if (ctrl.size() == 0 || ctrl.size() > 2)
        {
            return finish;
        }

        // 如果到达下限位，停止完成此动作
        if (downTarget())
        {
            finish = stop();
            DEBUG_STREAM_OUT("CIoLift:down target:key=" << ctrl[0].key
                                                      << ",set stop finish=" << finish);
            return finish;
        }

        // 控制停止输出
        if (ctrl.size() == 2)
        {
            ctrl[0].value = false;
            ctrl[1].value = true;
        }
        else
        {
            ctrl[0].value = true;
        }
        bool ret = outputIo(ctrl);
        DEBUG_STREAM_OUT("down:set io out key=" << ctrl[0].key << ",ret=" << ret);
        return finish;
    }

    // 举升停止
    bool CIoLift::stop()
    {
        bool finish = false;

        if (ctrl.size() == 0 || ctrl.size() > 2)
        {
            return finish;
        }

        // 控制停止输出
        if (ctrl.size() == 2)
        {
            ctrl[0].value = false;
            ctrl[1].value = false;
        }
        else
        {
            ctrl[0].value = false;
        }
        finish = outputIo(ctrl);

        if (finish)
        {
            DEBUG_WARN_OUT("stop lift ok");
        }
        return finish;
    }

    // 货物装填
    bool CIoLift::loadStatus()
    {
        bool load = false;
        // 如果不处在下限位，则一直报有货状态
        bool unload = downTarget() && !upTarget();
        load = !unload;
        return load;
    }

    // 上升到位
    bool CIoLift::upTarget()
    {
        bool ret = inputIo(upLimit);
        if (ret)
        {
            DEBUG_WARN_OUT("upTarget ok");
        }
        return ret;
    }

    // 下降到位
    bool CIoLift::downTarget()
    {
        bool ret = inputIo(downLimit);
        if (ret)
        {
            DEBUG_WARN_OUT("downTarget ok");
        }
        return ret;
    }

    // 控制IO点输出 param1:索引 param2:触发
    bool CIoLift::outputIo(IO_GROUP outIoGroup)
    {
        if (!m_client->service_is_ready()) {
            DEBUG_ERROR_OUT("Service client is not ready");
            return false;
        }

        if (m_outIoRecord == outIoGroup) {
            DEBUG_STREAM_OUT( "Same IO group as last command");
            return true;
        }

        auto request = std::make_shared<agv_srvs::srv::OutputsNew::Request>();
        request->array = outIoGroup;

        auto future_result = m_client->async_send_request(request);
        
        if (future_result.wait_for(std::chrono::seconds(5)) != std::future_status::ready) {
            DEBUG_ERROR_OUT("Service call timeout");
            return false;
        }

        auto response = future_result.get();
        if (response->state) {
            m_outIoRecord = outIoGroup;
            DEBUG_STREAM_OUT( "IO control success");
            return true;
        } else {
            DEBUG_ERROR_OUT("Service returned failure");
            return false;
        }
    }

    ///////////////////////////////CIoLifterController 实现///////////////////////////////////////////////
    // 默认构造函数
    CIoLifterController::CIoLifterController(std::shared_ptr<rclcpp::Node> n):
        node(n), m_init(false)
    {
    }

    // 析构函数
    CIoLifterController::~CIoLifterController()
    {
    }

    // 是否初始化完成
    bool CIoLifterController::initFinish()
    {
        if (!m_init)
        {
            DEBUG_ERROR_OUT("IoLifterController init failed");
        }
        return m_init;
    }

    // 初始化函数
    bool CIoLifterController::init()
    {
        // 订阅输入信号话题/readInput
        m_subIoNew = node->create_subscription<agv_msgs::msg::ReadInPutsNew>(
                "/readInputNew", 10,
                std::bind(&CIoLifterController::readInputNewCallBack, this, std::placeholders::_1)
            );

        m_outClient = node->create_client<agv_srvs::srv::OutputsNew>("/outputNew");

        int ioControlCount = 1;
        node->get_parameter_or<int>("IoCtrolCount", ioControlCount, 2);
        // 初始化io key-名字
        agv_msgs::msg::PairTypeInt io;
        io.value = 0;

        // 液压举升-如果数量为2 默认第一个升，第二个io为降
        io.key = UP_LIMIT;
        m_normalLift.upLimit.Update(io);
        io.key = DOWN_LIMIT;
        m_normalLift.downLimit.Update(io);

        io.key = CONTROL_IO_UP;
        m_normalLift.ctrl.push_back(io);
        if (ioControlCount == 2)
        {
            io.key = CONTROL_IO_DOWN;
            m_normalLift.ctrl.push_back(io);
        }

        // 急停按钮
        io.key = EMG_BUTTON;
        m_emgBtn.Update(io);

        io.key = EMG_BUTTON_2;
        m_emgBtn_2.Update(io);

        m_normalLift.init(m_outClient);

        m_init = true;
        return true;
    }

    // 举升上升
    bool CIoLifterController::up()
    {
        bool finish = false;
        if (!initFinish())
        {
            return finish;
        }

        // 控制下降输出
        bool ret = true;
        ret &= m_normalLift.up();
        DEBUG_STREAM_OUT("IoLifterController:up :"
                       << ",ret=" << ret);

        finish = ret;

        return finish;
    }

    // 举升下降
    bool CIoLifterController::down()
    {
        bool finish = false;
        if (!initFinish())
        {
            return finish;
        }

        // 控制下降输出
        bool ret = true;
        ret &= m_normalLift.down();
        DEBUG_STREAM_OUT("IoLifterController:down :"
                       << ",ret=" << ret);

        finish = ret;

        return finish;
    }

    // 所有举升停止
    bool CIoLifterController::stop()
    {
        bool finish = false;
        if (!initFinish())
        {
            return finish;
        }

        bool ret = true;
        ret &= m_normalLift.stop();
        DEBUG_STREAM_OUT("IoLifterController:stop :"
                       << ",ret=" << ret);
        finish = ret;

        if (finish)
        {
            DEBUG_WARN_OUT("stop lift ok");
        }
        return finish;
    }

    // 急停按下
    bool CIoLifterController::emgPressed()
    {
        bool ret = false;
        if (!initFinish())
        {
            return ret;
        }

        ret = (inputIo(m_emgBtn) || inputIo(m_emgBtn_2));
        if (ret)
        {
            DEBUG_WARN_OUT("emg preesed");
        }

        return ret;
    }

    // 货物装填
    bool CIoLifterController::loadStatus()
    {
        bool load = false;
        if (!initFinish())
        {
            return load;
        }

        // 液压举升不在最低处，防止撞工位，使用高度作为有无货物的其中一种条件
        load = m_normalLift.loadStatus();
        return load;
    }

    // 新更新输入IO对象
    void CIoLifterController::updateInutIoNew(const agv_msgs::msg::ReadInPutsNew &msg, INPUT_IO_TYPE &inputIoObj)
    {
        auto inputIo = inputIoObj.Get();
        std::vector<agv_msgs::msg::PairTypeInt> inputs_msg = msg.array;
        for (auto &data : inputs_msg)
        {
            if (data.key == inputIo.key)
            {
                inputIoObj.Update(data);
                // DEBUG_OUT("updateInutIoNew:inputIo.key=" << inputIo.key << ",data.value=" << data.value);
                break;
            }
        }
    }

    // 获取IO输入信号回调函数 io topic话题频率30hz
    void CIoLifterController::readInputNewCallBack(const agv_msgs::msg::ReadInPutsNew::ConstSharedPtr &msg)
    {
        // 获取举升上下限位IO信号 并更新数据的时间戳

        updateInutIoNew(*msg, m_normalLift.upLimit);
        updateInutIoNew(*msg, m_normalLift.downLimit);

        // // 获取急停的
        updateInutIoNew(*msg, m_emgBtn);
        updateInutIoNew(*msg, m_emgBtn_2);
    }
}; // end namespace
